#include "precompiled.h"
#include "Config.h"
#include "Util.h"
#include "Environment.h"

using namespace boost::numeric::ublas;

Environment::Environment(Robot *robot) :
	mDebug (Config::Debug),
	TimeStep (2.5e-4),
	EndTime (2.0),
	Width (500),
	Height (500),
	mResult (),
	mRobot (robot),
	mTrack (),
	mCarMat (4, 4)
{
	json::grammar<char>::object &o = Config::Get("Environment");
	Config::TryAssign(TimeStep, "TimeStep", o);
	Config::TryAssign(EndTime, "EndTime", o);
	Config::TryAssign(Width, "Width", o);
	Config::TryAssign(Height, "Height", o);
	
	robot->SetEnvironment(this);
}

Environment::~Environment() {
	
}

void Environment::RunSimulation() {
	vector<double> x_axis(2);
	x_axis[0] = 1.0;
	x_axis[1] = 0;
	
	mRobot->Start();
	mCarPos = mTrack.GetStartPosition();
	vec startHeading = mTrack.GetStartHeading();
	mDirection = atan2(startHeading[1], startHeading[0]);

	// main simulation loop
	int steps = (int)(EndTime / TimeStep);
	mResult.Resize(steps);
	int lastPercent = 0;

	mStep = 0;
	for(mStep = 0; mStep < steps; mStep++) {
		mTime = TimeStep * mStep;
		
		Step();
		Save();

		// show our progress by printing dots accross the screen
		int percent = 80 * mStep / steps;
		int numPeriods = percent - lastPercent;
		for(int i = 0; i < numPeriods; i++) {
			std::cout << ".";
		}
		lastPercent = percent;
	}
}

void Environment::Step() {
	if(mDebug) {
		std::cerr << "mTime:      " << mTime << "\n";
		std::cerr << "step:       " << mStep << "\n";
		std::cerr << "mCarPos:    " << mCarPos << "\n";
		std::cerr << "mDirection: " << 180.0 * mDirection / M_PI << "\n";
		std::cerr << "WheelAngle: " << 180.0 * mRobot->GetWheelAngle() / M_PI << "\n";
		std::cerr << "Speed:      " << mRobot->GetSpeed() << "\n";
	}
	
	UpdateCarMat();
	mRobot->Step(mTime, TimeStep);

	double delta = mRobot->GetWheelAngle();
	if(delta == 0) {
		// if the wheel angle is 0, then the turning radius is infinite.
		// therefore this is a special case
		vector<double> vel(2);
		vel[0] = cos(mDirection);
		vel[1] = sin(mDirection);
		mCarPos += vel * mRobot->GetSpeed() * TimeStep;
	} else {
		// compute the radius of the trajectory of the center of mass
		double b = mRobot->GetCenterOfMass();
		double l = mRobot->GetLength();
		double cot = 1.0 / tan(delta);
		double R = sqrt(b * b + l * l * cot * cot);
		// compute distance of rear axle from pivot point
		double R1 = sqrt(R * R - b * b);
		// compute angle of rotatation 
		double dTheta = mRobot->GetSpeed() * TimeStep / R;
		if(mDebug) {
			std::cerr << "R: " << R << "\n";
			std::cerr << "R1: " << R1 << "\n";
			std::cerr << "dTheta: " << dTheta << "\n";
		}
		// set up vector representing center of rear axle relative to pivot point
		vector<double> p1(4);
		p1[0] = R1;
		p1[1] = 0;
		p1[2] = 0;
		p1[3] = 1.0;
		matrix<double> Tr1 = Util::MakeRotate(dTheta, 0, 0, 1.0);
		// rotate around pivot point
		vector<double> dpos = prod(Tr1, p1);
		// translate relative to starting point
		dpos[0] -= R1;
		// if negative, reflect across x axis
		if(delta < 0) {
			dpos[0] = -dpos[0];
			dTheta = -dTheta;
		}
		// rotate again about starting point
		matrix<double> Tr2 = Util::MakeRotate(mDirection - M_PI / 2, 0, 0, 1.0);
		vector<double> dpos2 = prod(Tr2, dpos);
		if(mDebug) {
			std::cerr << "dpos: " << dpos << "\n";
			std::cerr << "dpos2: " << dpos2 << "\n";
		}
		// update position and direction
		mCarPos += project(dpos2, range(0, 2));
		mDirection += dTheta;
	}
}

void Environment::UpdateCarMat() {
	// rotate
	Util::MakeRotateInPlace(mCarMat, mDirection, 0, 0, 1.0);
	// set translate components
	mCarMat(0, 3) = mCarPos[0];
	mCarMat(1, 3) = mCarPos[1];
}

void Environment::Save() {
	mResult.Seek(mStep);
	SimResult::Data &data = mResult.Current();
	data.step = mStep;
	data.time = mTime;
	data.carPos = mCarPos;
	data.carDirection = mDirection;
	mRobot->Save(data);
}